#pragma once
#define ActionUnit_Version 20220911

/*--------------------------------------------------------------------------
Module  : ActionUnit.h
Author  : LQ
Created : 2020.2.21
brief   : 本类是对V5自带的motor_group类进行二次封装。
          根据多年参赛经验,去掉motor_group类一些冗余的功能,并添加更多实用的功能。
          完全理解ActionUnit类的定义需要较高的C++语法知识,
          读者可自行参考相关资料。
----------------------------------------------------------------------------*/
/*
change log:
【2020.11.1】增加is_all_units_relaxing()函数。
【2021.3.13】修复了_act_for_enc启动non_blocking_act_for_enc_service_cb_static的卡死bug
【2021.3.30】修改：ActionUnit 的act表现，修复次数过多不再启动后台线程的bug
【2021.6.12】修改：ActionUnit 的act_enc,支持后台跑编码器设定超时
【2021.12.12】新增：ActionUnit act_enc_routine_static_cb 新增堵转退出
【2022.4.14】修改：ActionUnit 编码器度数默认为：deg, 以前为:raw;
             新增：ActionUnit 马达掉线、脱轴监测
【2022.4.14】修改：set_voltage(0),set_speed(0) 调用mtr_stop() ，根据情况停转
【2022.5.26】新增：watt(总功率)
【2022.5.26】新增：显示screen开关
【2022.8.19】修补：get_enc逻辑，防止多马达AU,0号马达掉线而编码器只能返回0
【2022.8.22】新增：是否自动休眠; all_units_stop新增刹停模式参数
【2022.9.11】新增：马达端口设置冲突检查;  get_motor_name_with_side_suffix()函数;
【2022.9.11】新增：设置自定义的编码器数值setPosition(value, rotationUnits::deg)
*/
#include "../VirtualButton/_system_pen_color_font.h"
#include "string"
#include "vector"
#include "vex.h"

#define DEBUG_COUT                                                             \
  if (0)                                                                       \
  cout

class ActionUnit; // 前向声明
extern bool ctrl_screen_display_err_info;

// 马达异常时,遥控振动模式
#define rumble_motor_stalled ".-"
#define rumble_motor_uninstalled "..."
#define rumble_motor_shaft_disengaged "--"

// 动作单元的类定义,动作单元包含一个马达组
class ActionUnit {
  /*____________________________________________________________________________________________________________
   * >>>>>模块:  构造函数:使用C++模板实现
   * 可以添加多个马达
   _______________________________________________________________________________________________________________*/
private:
  brakeType set_brake_type = brakeType::brake; // 默认刹停模式
public:
  static vector<ActionUnit *> vec_ptr_all_instacnes; // 所有产生的实例的指针
  vex::motor_group mtr_group;                        // 内部马达组
  vector<vex::motor *> vec_ptr_motors; // 马达指针,用以保存各个马达
  string unit_name = "ActionUnit";
  bool b_screen_enc_disp = true; // 是否全局显示

private:
  // 存储马达指针到vec_ptr_motors的函数
  void _addMotorToVector() {
    // 启动马达检测线程
    static bool motors_installing_check_running = false;
    if (!motors_installing_check_running) {
      // 从建立实例开始就检查马达是否正确连接
      static thread t(motors_error_check_routine);
      motors_installing_check_running = true;
    }
  } // 递归结束函数

  template <typename... Arg> void _addMotorToVector(motor &m0, Arg &... m) {
    vec_ptr_motors.push_back(&m0);
    _addMotorToVector(m...);
  }

public:
  // 构造函数,所有参数都为马达
  template <typename... Arg>
  ActionUnit(motor &m0, Arg &... m) : mtr_group(m0, m...) {
    b_screen_enc_disp = false; // 不显示
    e_act_enc = event(act_enc_routine_static_cb, this);
    _addMotorToVector(m0, m...);
    vec_ptr_all_instacnes.push_back(this); // 保存本实例指针
    set_stopping(set_brake_type);          // 设置默认刹停模式
  }

  // 构造函数,第一个参数为动作单元名字,后续参数都为马达
  template <typename... Arg>
  ActionUnit(string n, Arg &... m) : mtr_group(m...), unit_name(n) {
    b_screen_enc_disp = true; // 显示
    e_act_enc = event(act_enc_routine_static_cb, this);
    _addMotorToVector(m...);
    vec_ptr_all_instacnes.push_back(this); // 保存本实例指针
    set_stopping(set_brake_type);          // 设置默认刹停模式
  }

  /*____________________________________________________________________________________________________________
   *>>>>>模块:  所有运动单元常规通用函数，对所有的运动单元都有效
   _______________________________________________________________________________________________________________*/
public:
  static void all_units_reset_enc(string except_au_1_name = "",
                                  string except_au_2_name = "",
                                  string except_au_3_name = "",
                                  string except_au_4_name = "");
  static void all_units_stop();
  static void all_units_stop(brakeType brake_type);
  static int get_instances_count() {
    return vec_ptr_all_instacnes.size();
  }                                       // 实例数量
  static int get_instance_motor_count() { // 马达总数
    int cnt_m = 0;
    for (auto pau : vec_ptr_all_instacnes)
      cnt_m += pau->get_motor_count();
    return cnt_m;
  }

  /*____________________________________________________________________________________________________________
   * >>>>>模块:  所有运动单元自动休眠相关，对所有的运动单元都有效
   * 使用公共类静态函数、变量
   _______________________________________________________________________________________________________________*/
private:
  static timer t_working;                      // 所有AU公用
  static const int time_auto_relaxing = 15000; // 机器自动无操作休息时间
  static bool b_relaxing_static;               // 是否正在休眠
public:
  static bool b_auto_relaxing_on; // 需要自动休眠

public:
  // 静态函数,用以控制默认stop()函数,所有实例的stop()均生效
  static void
  set_all_units_motor_stop_status_on_relaxing(bool relaxing_condition = true);
  static void
  set_all_units_motor_stop_status_on_working(bool working_condition = true);
  static bool is_all_units_relaxing() { return b_relaxing_static; }

  /*____________________________________________________________________________________________________________
   * >>>>>模块:  异常检测、端口冲突检测相关函数
   * 使用公共类静态函数、变量
  _______________________________________________________________________________________________________________*/

private:
  int shaft_disengaged_cnt = 0;            // 可能脱轴次数计数
  bool shaft_disengaged_detecting = false; // 默认不开启多马达联动的脱轴检测

public:
  void set_shaft_disengaged_detecting_working(bool status) { // 脱轴检测
    shaft_disengaged_detecting = status;
  };
  static void motors_error_check_routine(); // 马达连接、脱轴检测函数

  /*____________________________________________________________________________________________________________
   * >>>>>模块:  端口冲突检测相关函数
     非独立部件，则不会检测马达是否存在端口冲突，需要在检测前set_indepent_component(false)将其设置为非独立部件以关闭端口检测
   _______________________________________________________________________________________________________________*/
private:
  bool is_indepent_component = true; // 独立部件，会端口冲突检测
public:
  void set_indepent_component(bool indepent = false) {
    is_indepent_component = indepent;
  }
  static void motor_port_conflict_check(); // 马达接口冲突检查

  /*____________________________________________________________________________________________________________
   * >>>>>模块:  set和get马达参数的相关函数
   _______________________________________________________________________________________________________________*/
private:
  double set_voltage_pct = 0; // 设定的电压百分比
  double set_speed_pct = 0;   // 设定的速度百分比

  /*内部使用的数学函数*/
  int _sgn(double x) { // 符号函数
    if (x > 0)
      return 1;
    else if (x < 0)
      return -1;
    else
      return 0;
  }
  double _abs(double x) { return (((x) > 0) ? (x) : -(x)); } // 绝对值函数

public:
  // 获取内部motor_group变量mtr_group,用于特殊用途
  motor_group &get_motor_group() { return mtr_group; }

  void set_name(string name) { unit_name = name; }; // 设置名字
  // 获取含有左右前后的AU名字
  string get_name() { return unit_name; } // 名字的string类
  const char *get_name_c_str() {
    return unit_name.c_str();
  } // 名字的C语言字符串

  /*获取含有前后左右侧后缀的马达名字：
  ----非底盘：(单元名字中不含有"chassis" 和"chassis")
    1马达: 马达名字=单元名字
    多马达:
  0号马达名字=单元名字_l; 1号马达名字=单元名字_r; 2号马达名字=单元名字_m;
  n号马达名字=单元名字_n;

  ----底盘：(单元名字中含有"chassis" 和"chassis")
    1马达: 马达名字=单元名字
    多马达:
  0号马达名字=单元名字_f; 1号马达名字=单元名字_b; 2号马达名字=单元名字_m;
  n号马达名字=单元名字_n;
  */
  char *get_motor_name_with_side_suffix(int motor_index);
  int get_motor_count() { return mtr_group.count(); } // 获取马达的数量

  /*
    1.每转脉冲数:
    每转脉冲数是马达输出轴转1圈, 编码器的数值。
    定义每转脉冲数为rtpr(raw ticks per revolution)
     红齿 36:1 - rtpr=1800
     绿齿 18:1 - rtpr=900
     蓝齿  6:1 - rtpr=300

    2.单位转换公式:
    (raw) / rtpr =  (rev)
    (rev) * 360 =  (deg)
  */
  void set_enc(double value, rotationUnits unit = deg) {
    mtr_group.setPosition(value, unit);
  }

  double get_enc(int motor_index = 0, rotationUnits unit = rotationUnits::deg);
  void reset_enc();

  // 获取已经正确连接马达的下标,如果都没有正确连接，则返回default_index
  int get_installed_motor_index_with_default(int default_index);
  // 获取马达实际转速
  double get_actual_velocity(int motor_index = 0,
                             velocityUnits units_v = velocityUnits::pct);

  motor &get_first_motor() { return *vec_ptr_motors[0]; } // 获取第一个马达
  motor &get_motor(int index) {
    if (index > get_motor_count())
      index = 0;
    return *vec_ptr_motors[index];
  } // 获取指定马达

  double get_watt(); // 所有马达的功率之和

  // 根据设定马达电压百分百转动马达组,满电压为12.8伏
  void mtr_set_voltage(double pct_voltage);

  // 根据设定马达电压百分百转动马达组,满电压为12.8伏,并停止后台act_enc线程
  void set_voltage(double pct_voltage);

  // 根据数值和单位设定的电压转动马达组
  void set_voltage(double voltage, voltageUnits unit);

  double get_set_voltage_pct() { return set_voltage_pct; }
  double get_set_speed_pct() { return set_speed_pct; }
  double get_set_power_pct();
  double get_actual_velocity_pct(int motor_index = 0);
  /*
    根据设定马达速度百分百转动马达组
    红齿36:1 - 100rpm
    绿齿18:1 - 200rpm
    蓝齿 6:1 - 600rpm
 */
  void set_speed(double speed, velocityUnits unit = velocityUnits::pct);

  void set_stopping(brakeType brake_type); // 设定刹停模式
  void mtr_stop();                         // 使用设定的刹停模式刹停
  void mtr_stop(brakeType brake_type); // 根据参数指定的刹停模式临时刹停

  // 使用设定的刹停模式刹停,并关闭所有后台线程,【阻塞型】函数
  void stop();
  // 使用参数指定的刹停模式刹停,并关闭所有后台线程,【阻塞型】函数
  void stop(brakeType brake_type);

  // 临时刹停
  void brake_stop();
  void coast_stop();
  void hold_stop();

  /*____________________________________________________________________________________________________________
   * >>>>>模块:  跑编码器act_for_enc相关的函数
   _______________________________________________________________________________________________________________*/
public:
  void act_for_enc_absolute(double aim_enc, double speed, int timeout_ms,
                            Block waitForCompletion = Block::yes,
                            bool wait_for_former_done = false,
                            int delay_ending_time = 0,
                            rotationUnits units_r = rotationUnits::deg,
                            velocityUnits units_v = velocityUnits::pct);
  void act_for_enc_relative(double aim_enc, double speed, int timeout_ms,
                            Block waitForCompletion = Block::yes,
                            bool wait_for_former_done = false,
                            int delay_ending_time = 0,
                            rotationUnits units_r = rotationUnits::deg,
                            velocityUnits units_v = velocityUnits::pct);
  bool is_act_enc_running();

  // 跑编码器相关的底层函数
private:
  struct {
    bool absolute_mode;
    double aim_enc;
    rotationUnits units_r;
    double speed;
    velocityUnits units_v;
    int timeout;
    int delay_ending_time;

    bool act_enc_force_stop;
    bool act_enc_running;
  } data_act_enc = {
      false, 0, rotationUnits::deg, 0, velocityUnits::pct, 0, 0, false, false};

  event e_act_enc;

  void stop_act_enc();
  static void act_enc_routine_static_cb(void *p_this); // 后台实际运行线程函数
  void _act_for_enc(bool absolute_mode, double aim_enc, double speed,
                    int timeout_ms, Block waitForCompletion,
                    bool wait_for_former_done, int delay_ending_time,
                    rotationUnits units_r, velocityUnits units_v);
  /*____________________________________________________________________________________________________________*/
};
